/**
 * @file carto_mapping_test.cc
 * @author DustinKsi (dustinksi@126.com)
 * @brief
 * @version 0.1
 * @date 2019-12-17
 *
 * @copyright Copyright (c) 2019
 *
 */

#include "gmock/gmock.h"
#include "gtest/gtest.h"

#include "venus/mapping/carto_mapping/carto_mapping.h"

namespace VEnus {
namespace Mapping {

TEST(CartoMappingTest, SiftNewFeaturePoints) {
  MappingOptions options;
  options.set_distance_ceil_threshold(12.0);
  options.set_distance_match_tolerance(0.3);

  CartoMapping map_;
  map_.SetMappingOptions(options);

  cv::RNG rng;

  VEnus::Sensor::Feature2DList feature_list_first_frame;
  vector<pair<double, double> > fpts_list_first_frame;

  fpts_list_first_frame.push_back(make_pair(0.0+ rng.uniform(0.0f, 0.05f), 3.0+ rng.uniform(0.0f, 0.05f)));
  fpts_list_first_frame.push_back(make_pair(4.0+ rng.uniform(0.0f, 0.05f), 0.0+ rng.uniform(0.0f, 0.05f)));
  fpts_list_first_frame.push_back(make_pair(4.0+ rng.uniform(0.0f, 0.05f), 3.0+ rng.uniform(0.0f, 0.05f)));
  // fpts_list_first_frame.push_back(make_pair(7.0, 8.0));

  VEnus::Sensor::Feature2DWithID* nwfpt;
  for (auto fpt : fpts_list_first_frame) {
    nwfpt = feature_list_first_frame.add_feature_2d_with_ids();
    nwfpt->set_x(fpt.first);
    nwfpt->set_y(fpt.second);
  }

  VEnus::Sensor::Feature2DList feature_list_second_frame;
  vector<pair<double, double> > fpts_list_second_frame;
  fpts_list_second_frame.push_back(make_pair(9.4284, 0.3300));
  fpts_list_second_frame.push_back(
      make_pair(-0.7320 + rng.uniform(0.f, 0.05f), 2.7321 + rng.uniform(0.f, 0.05f)));
  fpts_list_second_frame.push_back(
      make_pair(1.2321 + rng.uniform(0.f, 0.05f), -1.8661 + rng.uniform(0.f, 0.05f)));
  fpts_list_second_frame.push_back(
      make_pair(2.7321 + rng.uniform(0.f, 0.05f), 0.7320 + rng.uniform(0.f, 0.05f)));

  for (auto fpt : fpts_list_second_frame) {
    nwfpt = feature_list_second_frame.add_feature_2d_with_ids();
    nwfpt->set_x(fpt.first);
    nwfpt->set_y(fpt.second);
  }

  vector<pair<double, double> > fpts_list_third_frame;
  fpts_list_third_frame.push_back(
      make_pair(0.4146 + rng.uniform(0.0f, 0.05f), 7.3429 + rng.uniform(0.0f, 0.05f)));
  fpts_list_third_frame.push_back(
      make_pair(3.5019 + rng.uniform(0.0f, 0.05f), 3.4098 + rng.uniform(0.0f, 0.05f)));
  fpts_list_third_frame.push_back(
      make_pair(4.2783 + rng.uniform(0.0f, 0.05f), 6.3076 + rng.uniform(0.0f, 0.05f)));
  fpts_list_third_frame.push_back(
      make_pair(10.8503 + rng.uniform(0.0f, 0.05f), 7.6525 + rng.uniform(0.0f, 0.05f)));

  VEnus::Sensor::Feature2DList feature_list_third_frame;
  for (auto fpt : fpts_list_third_frame) {
    nwfpt = feature_list_third_frame.add_feature_2d_with_ids();
    nwfpt->set_x(fpt.first);
    nwfpt->set_y(fpt.second);
  }

  VEnus::Sensor::AdjacencyList result;
  VEnus::Sensor::RobotPose pose;
  // map_.InsertFeatureList(feature_list_first_frame, result, pose);
  // map_.InsertFeatureList(feature_list_second_frame, result, pose);
  // map_.InsertFeatureList(feature_list_third_frame, result, pose);

  
}

TEST(CartoMappingTest, FakeMapBuild) {
  MappingOptions options;
  options.set_distance_ceil_threshold(12.0);
  options.set_distance_match_tolerance(0.3);

  CartoMapping map_;
  map_.SetMappingOptions(options);

  cv::RNG rng;

  VEnus::Sensor::Feature2DList feature_list_first_frame;
  vector<pair<double, double> > fpts_list_first_frame;

  fpts_list_first_frame.push_back(make_pair(0.494567, -1.67772));
  fpts_list_first_frame.push_back(make_pair(0.565315, 1.01885));
  fpts_list_first_frame.push_back(make_pair(-4.02344, 1.84843));
  // fpts_list_first_frame.push_back(make_pair(7.0, 8.0));

  VEnus::Sensor::Feature2DWithID* nwfpt;
  for (auto fpt : fpts_list_first_frame) {
    nwfpt = feature_list_first_frame.add_feature_2d_with_ids();
    nwfpt->set_x(fpt.first);
    nwfpt->set_y(fpt.second);
  }

 VEnus::Sensor::AdjacencyList result;
  VEnus::Sensor::RobotPose pose;
  map_.InsertFeatureList(feature_list_first_frame, result, pose);

  
}

}  // namespace Mapping
}  // namespace VEnus

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  FLAGS_stderrthreshold = google::INFO;
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}